#include "task.h"

s16 PulseBase[6];  //步进电机当前相对于起点的脉冲数
s16 PulseTarget[6];  //步进电机的目标脉冲数
s8 dir[6];  //步进电机的目标方向
s16 HalfWidth[6];  //步进电机PWM的半周期=HalfWidth[i]*10us
u16 MaxSpeed[6];  //最大速度限制,单位为脉冲/秒(p/s)
u16 MaxAcc[6];  //最大加速度限制,单位为脉冲/平方秒(p/s^2)
u16 tempMaxSpeed[6];  //校准时将当前设置的最大速度暂存
u8 speedstore=0;  //暂存标志位
u8 CalibrateFlag=0;  //哪个关节需要校准
float TDx1[6]={0, 0, 0, 0, 0, 0};  //跟踪微分器输出跟踪信号
float TDx2[6]={0, 0, 0, 0, 0, 0};  //跟踪微分器输出微分信号
u8 AdcFlag=0;

/**********************
参数初始化
**********************/
void Param_Init(void)
{
    u8 i;
    for(i=0;i<6;i++){
        PulseBase[i]=0;
        PulseTarget[i]=0;
        dir[i]=0;
        HalfWidth[i]=1;
        MaxSpeed[i]=1000;
        MaxAcc[i]=500;
    }
}

/**********************
离散系统最速控制综合函数
*@x1:二阶串联积分器的输出
*@x2:二阶串联积分器输出的微分
*@return:控制量输出
**********************/
float ADRC_fhan(float x1, float x2, float R, float V)
{
    float u;
    float y=x1+H*x2;
    float absy=ABS(y);
    float d1=1/(H*H*R);
    float k=(1+Msqrt(1+8*d1*absy))*0.5;
    k=SIGN(k-(int)k)+(int)k;
    if(absy*d1<=1)
        u=-(y+H*x2)*d1;
    else
        u=SIGN(y)*(1-0.5*k)-(x1+k*H*x2)/(k-1)*d1;
    u=R*LIMIT(u,-1,1);
    absy=ABS(x2+H*u);
    if(absy>V){
        u=(V*SIGN(x2)-x2)/H;
        u=LIMIT(u,-R,R);
    }
    return u;
}

/**********************
20ms控制周期
**********************/
void Control_Loop(void)
{
    u8 i;
    float u;
    for(i=0; i<6; ++i){
        u=ADRC_fhan(PulseBase[i]-PulseTarget[i], TDx2[i], MaxAcc[i], MaxSpeed[i]);
        TDx1[i] += H*TDx2[i];
        TDx2[i] += H*u;
        if((ABS(TDx2[i])<=3.06) || (PulseBase[i]==PulseTarget[i])){
            dir[i] = TDx2[i] = 0;
            continue;
        }
        HalfWidth[i] = ABS(50000/TDx2[i]) + 0.5;
        dir[i] = (TDx2[i]>0) ? 1 : -1;
    }
    if (dir[0] && !(TIM2->CR1 & TIM_CR1_CEN)) {TIM2->ARR = HalfWidth[0]; TIM2->CR1 |= TIM_CR1_CEN;}
    if (dir[1] && !(TIM3->CR1 & TIM_CR1_CEN)) {TIM3->ARR = HalfWidth[1]; TIM3->CR1 |= TIM_CR1_CEN;}
    if (dir[2] && !(TIM4->CR1 & TIM_CR1_CEN)) {TIM4->ARR = HalfWidth[2]; TIM4->CR1 |= TIM_CR1_CEN;}
    if (dir[3] && !(TIM5->CR1 & TIM_CR1_CEN)) {TIM5->ARR = HalfWidth[3]; TIM5->CR1 |= TIM_CR1_CEN;}
    if (dir[4] && !(TIM6->CR1 & TIM_CR1_CEN)) {TIM6->ARR = HalfWidth[4]; TIM6->CR1 |= TIM_CR1_CEN;}
    if (dir[5] && !(TIM7->CR1 & TIM_CR1_CEN)) {TIM7->ARR = HalfWidth[5]; TIM7->CR1 |= TIM_CR1_CEN;}
}

/**********************
生成PWM脉冲
**********************/
s8 PWM_Generate(int n, s16 *period)
{
    static u8 isHigh=0xFF;  // 上升沿或下降沿标志位
    if (isHigh & (1<<n)){  // 上升沿标志一个脉冲结束
        MOTOR_PWM_OUTPUT_HIGH(n);
        if(dir[n]==-1) MOTOR_DIR_OUTPUT_LOW(n);
        else MOTOR_DIR_OUTPUT_HIGH(n);  // 停转或正转设置为正方向
        PulseBase[n]+=dir[n];  // 脉冲步数更新
        *period = HalfWidth[n];  // 设置下一次脉冲的脉宽
        if (dir[n]) isHigh ^= (1<<n);
        return dir[n];
    }
    else{  // 下降沿触发脉冲,此时不可修改脉宽或方向
        MOTOR_PWM_OUTPUT_LOW(n);
        *period = 0;  // 下一次脉宽为0表示下降沿不修改脉宽
        isHigh ^= (1<<n);
        return 1;  // 定时器保持工作状态
	}
}

/**********************
收到校准指令后执行一次
**********************/
void Start_Joint_Calibrate(void)
{
    u8 i;
    for(i=0;i<6;++i){
        if(!(CalibrateFlag&(1<<i)))continue;
        PulseTarget[i]=-20000;  // 将目标脉冲数设到负无穷
        if (speedstore & (1<<i)) continue;
        tempMaxSpeed[i]=MaxSpeed[i];  // 最大速度暂存以便校准结束后恢复
        speedstore |= (1<<i);
        MaxSpeed[i]=CALIB_MAXSPEED;  // 降低最大速度
    }
}

/**********************
5ms执行一次,用于校准过程中检测是否触碰限位开关
以最大速度为100p/s为例，5ms移动0.5p，不会产生多余脉冲
**********************/
void Joint_Task_5ms(void)
{
    u8 i;
    u16 idr = SENSOR_PORT->IDR & SENSOR_PIN;
    if(!CalibrateFlag) return;  //未在校准
    if(idr == SENSOR_PIN) return;  //未触碰限位开关
    for(i=0;i<6;++i){
        if(!(CalibrateFlag & (1<<i))) continue;  //第i个关节未在校准
        if(idr & SensorPin[i]) continue;  //第i个关节未触碰限位开关
        PulseBase[i] = -10;  //将触碰限位开关前的一个步数设为起点
        PulseTarget[i] = 0;  //将目标步数设为起点
        Send_u8_data((1<<i), 0x80, DMA_Buffer_Stuff);  //向上位机发送校准完毕信号
        MaxSpeed[i] = tempMaxSpeed[i];  //恢复校准之前设置的最大速度
        speedstore &=~ (1<<i);  //已恢复最大速度
        CalibrateFlag &=~ (1<<i);  //将当前关节标志为未在校准
        LED_ON;
    }
    DMA_Enable();
}

void Send_Adc(void)
{
    u16 AdcData;
    if (!AdcFlag) return;
    AdcData = Get_Adc();
    Send_u16_data(&AdcData, REQ_ADC, 1, DMA_Buffer_Stuff);
    DMA_Enable();
}

/**********************
串口接收数据处理
**********************/
void Data_Receive(void)
{
    u8 i,u8data;
    u16 u16data;
    switch(Fcn){  //接收到指令
    case PULSE_TARGET:  //目标脉冲数
        for(i=0;i<6;++i)
            PulseTarget[i] = (RxTemp[i<<1]<<8) + RxTemp[(i<<1)+1];
        LED_ON;
        speedstore = CalibrateFlag = 0;  //校准未完成时收到指令则放弃校准
        break;
    case PULSE_RESET:  // 步数复位
        u8data = RxTemp[0];
        for (i=0; i<6; ++i){
            if (u8data & (1<<i))
                PulseBase[i] = PulseTarget[i] = 0;
        }
        if (u8data & 0x80){
            for (i=0; i<6; ++i){
                PulseTarget[i] = PulseBase[i];
                if (!(u8data & 0x40)) continue;
                TDx1[i] = TDx2[i] = 0;
                dir[i] = 0;
            }
        }
        break;
    case REQ_ADC:  //压力采集
        u8data = RxTemp[0];
        AdcFlag = u8data & 0x01;
        break;
    case MAX_SPEED:  //最小半脉宽
        for(i=0;i<6;++i){
            u16data = (RxTemp[i<<1]<<8) + RxTemp[(i<<1)+1];
            MaxSpeed[i] = LIMIT(u16data, 5, 10000);
        }
        break;
    case MAX_ACC:  //最大加速度
        for(i=0;i<6;++i){
            u16data = (RxTemp[i<<1]<<8) + RxTemp[(i<<1)+1];
            MaxAcc[i] = LIMIT(u16data, 5, 10000);
        }
        break;
    case REQUIRE:  // 请求参数
        u8data = RxTemp[0];
        if(u8data&0x01){  //获取最小半脉宽
            Send_u16_data(MaxSpeed, 0x10, 6, DMA_Buffer_Stuff);
            DMA_Enable();
        }
        if(u8data&0x02){  //获取最大加速度
            Send_u16_data(MaxAcc, 0x20, 6, DMA_Buffer_Stuff);
            DMA_Enable();
        }
        if(u8data&0xFC){  //校准
            CalibrateFlag |= u8data>>2;
            Start_Joint_Calibrate();
            LED_OFF;
        }
        break;
    default:break;
    }
}
